/* Programa de prueba */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/termios.h>
#include <fcntl.h>
#include <unistd.h>


#define MAGIC_CODE 0xf3

struct servctl {
	unsigned char magic;
	unsigned char operation;
	unsigned short param;
};

enum OPERATION { SET_WIDTH=1, DO_PULSE };

void servctl_set_width(int fd, unsigned short width)
{
	struct servctl sc;

	sc.magic = MAGIC_CODE;
	sc.operation = SET_WIDTH;
	sc.param = width;

	write(fd, &sc, sizeof(sc));
}

void servctl_do_pulse(int fd, unsigned short times)
{
	struct servctl sc;
	char ret;

	sc.magic = MAGIC_CODE;
	sc.operation = DO_PULSE;
	sc.param = times;

	write(fd, &sc, sizeof(sc));
	read(fd, &ret, 1);
	if(ret != 1)
		puts("do_pulse error");

}

int main(int c, char **v)
{
        struct termios term, new_topts, old_topts;
	int fd;
	unsigned short i;
	char in;


	if(c < 2)
		return 1;

	fd = open(v[1], O_RDWR);
	if(fd == -1) {
		perror("open");
		return 1;
	}

	if(tcgetattr(fd, &term) == -1) {
		perror("tcgetattr");
		exit(EXIT_FAILURE);
	}

	cfmakeraw(&term);
	term.c_cflag |= CREAD;
	term.c_cc[VTIME] = 0;
	term.c_cc[VMIN] = 1;
	cfsetispeed(&term, B9600);

	tcflush(fd, TCIFLUSH);
	if(tcsetattr(fd, TCSANOW, &term) == -1) {
		perror("tcsetattr");
		exit(EXIT_FAILURE);
        }
/*
	tcgetattr(0, &old_topts);
	memcpy(&new_topts, &old_topts, sizeof(new_topts));
	new_topts.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ECHOPRT | ECHOKE | ICRNL);
	tcsetattr(0, TCSANOW, &new_topts);
*/
/*	while(read(0, &in, 1) == 1) {
		switch(in) {
			case 'a':
				sc.magic = MAGIC_CODE;
				sc.high_period = 400;
				write(fd, &sc, sizeof(struct servctl));
				break;
			case 'd':
				sc.magic = MAGIC_CODE;
				sc.high_period = 2500;
				write(fd, &sc, sizeof(struct servctl));
				break;
			case 's':
				tcsetattr(0, TCSANOW, &old_topts);
				puts("Ingresa delay:");
				scanf("%hu", &custom);
				tcsetattr(0, TCSANOW, &new_topts);
			case 'c':
				sc.magic = MAGIC_CODE;
				sc.high_period = custom;
				write(fd, &sc, sizeof(struct servctl));
				break;
		 }
	}
*/

/* test test test test test */

	for(i = 10; i < 2500; i+=50) {
		fprintf(stdout, "delay=%hu\n", i);
		servctl_set_width(fd, i);
		servctl_do_pulse(fd, 500);

		usleep(300000);
	}

/* Hasta el momento la prueba arroja que entre 1410 y 1460 esta el cambio de direccion */
/* ************************* */

	return 0;
}
